﻿using System;

namespace CommonFramework.FixMath
{
    public struct FQuaternion
    {
        #region public members

        public FFloat X;
        public FFloat Y;
        public FFloat Z;
        public FFloat W;

        #endregion

        #region constructor

        public FQuaternion(FFloat x, FFloat y, FFloat z, FFloat w)
        {
            X = x;
            Y = y;
            Z = z;
            W = w;
        }
        #endregion

        #region public properties

        public FFloat this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                    return X;
                    case 1:
                    return Y;
                    case 2:
                    return Z;
                    case 3:
                    return W;
                    default:
                    throw new IndexOutOfRangeException("Invalid LQuaternion index!");
                }
            }
            set
            {
                switch (index)
                {
                    case 0:
                    X = value;
                    break;
                    case 1:
                    Y = value;
                    break;
                    case 2:
                    Z = value;
                    break;
                    case 3:
                    W = value;
                    break;
                    default:
                    throw new IndexOutOfRangeException("Invalid LQuaternion index!");
                }
            }
        }

        public readonly static FQuaternion Identity = 
            new FQuaternion(FFloat.FromRawInt(0), FFloat.FromRawInt(0), FFloat.FromRawInt(0), FFloat.FromRawInt(1));

        public FVector3 eulerAngles
        {
            get
            {
                FMatrix33 m = QuaternionToMatrix(this);
                return (180 / FMathUtil.PI * MatrixToEuler(m));
            }
            set { this = Euler(value); }
        }

        #endregion

        #region public functions

        /// <summary>
        /// 夹角大小
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static FFloat Angle(FQuaternion a, FQuaternion b)
        {
            FFloat single = Dot(a, b);
            return FMathUtil.Acos(FMathUtil.Min(FMathUtil.Abs(single), FFloat.One)) * 2 * (180 / FMathUtil.PI);
        }

        /// <summary>
        /// 轴向旋转
        /// </summary>
        /// <param name="angle"></param>
        /// <param name="axis"></param>
        /// <returns></returns>
        public static FQuaternion AngleAxis(FFloat angle, FVector3 axis)
        {
            axis = axis.Normalized;
            angle = angle * FMathUtil.Deg2Rad;

            FQuaternion q = new FQuaternion();

            FFloat halfAngle = angle * FFloat.Half;
            FFloat s = FMathUtil.Sin(halfAngle);

            q.W = FMathUtil.Cos(halfAngle);
            q.X = s * axis.X;
            q.Y = s * axis.Y;
            q.Z = s * axis.Z;

            return q;
        }

        /// <summary>
        /// 点乘
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static FFloat Dot(FQuaternion a, FQuaternion b)
        {
            return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
        }

        /// <summary>
        /// 欧拉角转四元数
        /// </summary>
        /// <param name="euler"></param>
        /// <returns></returns>
        public static FQuaternion Euler(FVector3 euler)
        {
            return Euler(euler.X, euler.Y, euler.Z);
        }

        /// <summary>
        /// 欧拉角转四元数
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="z"></param>
        /// <returns></returns>
        public static FQuaternion Euler(FFloat x, FFloat y, FFloat z)
        {
            FFloat cX = FMathUtil.Cos(x * FMathUtil.PI / 360);
            FFloat sX = FMathUtil.Sin(x * FMathUtil.PI / 360);

            FFloat cY = FMathUtil.Cos(y * FMathUtil.PI / 360);
            FFloat sY = FMathUtil.Sin(y * FMathUtil.PI / 360);

            FFloat cZ = FMathUtil.Cos(z * FMathUtil.PI / 360);
            FFloat sZ = FMathUtil.Sin(z * FMathUtil.PI / 360);

            FQuaternion qX = new FQuaternion(sX, FFloat.Zero, FFloat.Zero, cX);
            FQuaternion qY = new FQuaternion(FFloat.Zero, sY, FFloat.Zero, cY);
            FQuaternion qZ = new FQuaternion(FFloat.Zero, FFloat.Zero, sZ, cZ);

            FQuaternion q = (qY * qX) * qZ;

            return q;
        }

        /// <summary>
        /// 向量间的角度
        /// </summary>
        /// <param name="fromDirection"></param>
        /// <param name="toDirection"></param>
        /// <returns></returns>
        public static FQuaternion FromToRotation(FVector3 fromDirection, FVector3 toDirection)
        {
            throw new IndexOutOfRangeException("Not Available!");
        }

        /// <summary>
        /// 四元数的逆
        /// </summary>
        /// <param name="rotation"></param>
        /// <returns></returns>
        public static FQuaternion Inverse(FQuaternion rotation)
        {
            return new FQuaternion(-rotation.X, -rotation.Y, -rotation.Z, rotation.W);
        }

        /// <summary>
        /// 线性插值
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="t"></param>
        /// <returns></returns>
        public static FQuaternion Lerp(FQuaternion a, FQuaternion b, FFloat t)
        {
            if (t > 1)
            {
                t = FFloat.One;
            }

            if (t < 0)
            {
                t = FFloat.Zero;
            }

            return LerpUnclamped(a, b, t);
        }

        /// <summary>
        /// 线性插值(无限制)
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="t"></param>
        /// <returns></returns>
        public static FQuaternion LerpUnclamped(FQuaternion a, FQuaternion b, FFloat t)
        {
            FQuaternion tmpQuat = new FQuaternion();
            if (Dot(a, b) < 0)
            {
                tmpQuat.Set(a.X + t * (-b.X - a.X),
                    a.Y + t * (-b.Y - a.Y),
                    a.Z + t * (-b.Z - a.Z),
                    a.W + t * (-b.W - a.W));
            }
            else
            {
                tmpQuat.Set(a.X + t * (b.X - a.X),
                    a.Y + t * (b.Y - a.Y),
                    a.Z + t * (b.Z - a.Z),
                    a.W + t * (b.W - a.W));
            }

            FFloat nor = FMathUtil.Sqrt(Dot(tmpQuat, tmpQuat));
            return new FQuaternion(tmpQuat.X / nor, tmpQuat.Y / nor, tmpQuat.Z / nor, tmpQuat.W / nor);
        }

        /// <summary>
        /// 注视旋转
        /// </summary>
        /// <param name="forward"></param>
        /// <returns></returns>
        public static FQuaternion LookRotation(FVector3 forward)
        {
            FVector3 up = FVector3.Up;
            return LookRotation(forward, up);
        }

        /// <summary>
        /// 注视旋转
        /// </summary>
        /// <param name="forward"></param>
        /// <param name="upwards"></param>
        /// <returns></returns>
        public static FQuaternion LookRotation(FVector3 forward, FVector3 upwards)
        {
            FMatrix33 m = LookRotationToMatrix(forward, upwards);
            return MatrixToQuaternion(m);
        }

        /// <summary>
        /// 向目标角度旋转
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <param name="maxDegreesDelta"></param>
        /// <returns></returns>
        public static FQuaternion RotateTowards(FQuaternion from, FQuaternion to, FFloat maxDegreesDelta)
        {
            FFloat num = FQuaternion.Angle(from, to);
            FQuaternion result = new FQuaternion();
            if (num == 0)
            {
                result = to;
            }
            else
            {
                FFloat t = FMathUtil.Min(FFloat.One, maxDegreesDelta / num);
                result = FQuaternion.SlerpUnclamped(from, to, t);
            }

            return result;
        }

        /// <summary>
        /// 球形插值
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="t"></param>
        /// <returns></returns>
        public static FQuaternion Slerp(FQuaternion a, FQuaternion b, FFloat t)
        {
            if (t > 1)
            {
                t = FFloat.One;
            }

            if (t < 0)
            {
                t = FFloat.Zero;
            }

            return SlerpUnclamped(a, b, t);
        }

        /// <summary>
        /// 球形插值(无限制)
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="t"></param>
        /// <returns></returns>
        public static FQuaternion SlerpUnclamped(FQuaternion q1, FQuaternion q2, FFloat t)
        {
            FFloat dot = Dot(q1, q2);

            FQuaternion tmpQuat = new FQuaternion();
            if (dot < 0)
            {
                dot = -dot;
                tmpQuat.Set(-q2.X, -q2.Y, -q2.Z, -q2.W);
            }
            else
                tmpQuat = q2;


            if (dot < 1)
            {
                FFloat angle = FMathUtil.Acos(dot);
                FFloat sinadiv, sinat, sinaomt;
                sinadiv = 1 / FMathUtil.Sin(angle);
                sinat = FMathUtil.Sin(angle * t);
                sinaomt = FMathUtil.Sin(angle * (1 - t));
                tmpQuat.Set((q1.X * sinaomt + tmpQuat.X * sinat) * sinadiv,
                    (q1.Y * sinaomt + tmpQuat.Y * sinat) * sinadiv,
                    (q1.Z * sinaomt + tmpQuat.Z * sinat) * sinadiv,
                    (q1.W * sinaomt + tmpQuat.W * sinat) * sinadiv);
                return tmpQuat;
            }
            else
            {
                return Lerp(q1, tmpQuat, t);
            }
        }

        /// <summary>
        /// 设置四元数
        /// </summary>
        /// <param name="new_x"></param>
        /// <param name="new_y"></param>
        /// <param name="new_z"></param>
        /// <param name="new_w"></param>
        public void Set(FFloat new_x, FFloat new_y, FFloat new_z, FFloat new_w)
        {
            X = new_x;
            Y = new_y;
            Z = new_z;
            W = new_w;
        }

        /// <summary>
        /// 设置角度
        /// </summary>
        /// <param name="fromDirection"></param>
        /// <param name="toDirection"></param>
        public void SetFromToRotation(FVector3 fromDirection, FVector3 toDirection)
        {
            this = FromToRotation(fromDirection, toDirection);
        }

        /// <summary>
        /// 设置注视旋转
        /// </summary>
        /// <param name="view"></param>
        public void SetLookRotation(FVector3 view)
        {
            this = LookRotation(view);
        }

        /// <summary>
        /// 设置注视旋转
        /// </summary>
        /// <param name="view"></param>
        /// <param name="up"></param>
        public void SetLookRotation(FVector3 view, FVector3 up)
        {
            this = LookRotation(view, up);
        }

        /// <summary>
        /// 转换为角轴
        /// </summary>
        /// <param name="angle"></param>
        /// <param name="axis"></param>
        public void ToAngleAxis(out FFloat angle, out FVector3 axis)
        {
            angle = 2 * FMathUtil.Acos(W);
            if (angle == 0)
            {
                axis = FVector3.Right;
                return;
            }

            FFloat div = 1 / FMathUtil.Sqrt(1 - W * W);
            axis = FVector3.FromFFloat(X * div, Y * div, Z * div);
            angle = angle * 180 / FMathUtil.PI;
        }

        public override string ToString()
        {
            return String.Format("({0}, {1}, {2}, {3})", X, Y, Z, W);
        }

        public override int GetHashCode()
        {
            return this.X.GetHashCode() ^ this.Y.GetHashCode() << 2 ^ this.Z.GetHashCode() >> 2 ^
                   this.W.GetHashCode() >> 1;
        }

        public override bool Equals(object other)
        {
            return this == (FQuaternion)other;
        }

        #endregion

        #region private functions

        private FVector3 MatrixToEuler(FMatrix33 m)
        {
            FVector3 v = new FVector3();
            if (m[1, 2] < 1)
            {
                if (m[1, 2] > -1)
                {
                    v.X = FMathUtil.Asin(-m[1, 2]);
                    v.Y = FMathUtil.Atan2(m[0, 2], m[2, 2]);
                    v.Z = FMathUtil.Atan2(m[1, 0], m[1, 1]);
                }
                else
                {
                    v.X = FMathUtil.PI * FFloat.Half;
                    v.Y = FMathUtil.Atan2(m[0, 1], m[0, 0]);
                    v.Z = (FFloat)0;
                }
            }
            else
            {
                v.X = -FMathUtil.PI * FFloat.Half;
                v.Y = FMathUtil.Atan2(-m[0, 1], m[0, 0]);
                v.Z = (FFloat)0;
            }

            for (int i = 0; i < 3; i++)
            {
                if (v[i] < 0)
                {
                    v[i] += FMathUtil.TwoPI;
                }
                else if (v[i] > FMathUtil.TwoPI)
                {
                    v[i] -= FMathUtil.TwoPI;
                }
            }

            return v;
        }

        public static FMatrix33 QuaternionToMatrix(FQuaternion quat)
        {
            FMatrix33 m = new FMatrix33();

            FFloat x = quat.X * 2;
            FFloat y = quat.Y * 2;
            FFloat z = quat.Z * 2;
            FFloat xx = quat.X * x;
            FFloat yy = quat.Y * y;
            FFloat zz = quat.Z * z;
            FFloat xy = quat.X * y;
            FFloat xz = quat.X * z;
            FFloat yz = quat.Y * z;
            FFloat wx = quat.W * x;
            FFloat wy = quat.W * y;
            FFloat wz = quat.W * z;

            m[0] = 1 - (yy + zz);
            m[1] = xy + wz;
            m[2] = xz - wy;

            m[3] = xy - wz;
            m[4] = 1 - (xx + zz);
            m[5] = yz + wx;

            m[6] = xz + wy;
            m[7] = yz - wx;
            m[8] = 1 - (xx + yy);

            return m;
        }

        private static FQuaternion MatrixToQuaternion(FMatrix33 m)
        {
            FQuaternion quat = new FQuaternion();

            FFloat fTrace = m[0, 0] + m[1, 1] + m[2, 2];
            FFloat root;

            if (fTrace > 0)
            {
                root = FMathUtil.Sqrt(fTrace + 1);
                quat.W = FFloat.Half * root;
                root = FFloat.Half / root;
                quat.X = (m[2, 1] - m[1, 2]) * root;
                quat.Y = (m[0, 2] - m[2, 0]) * root;
                quat.Z = (m[1, 0] - m[0, 1]) * root;
            }
            else
            {
                int[] s_iNext = new int[] { 1, 2, 0 };
                int i = 0;
                if (m[1, 1] > m[0, 0])
                {
                    i = 1;
                }

                if (m[2, 2] > m[i, i])
                {
                    i = 2;
                }

                int j = s_iNext[i];
                int k = s_iNext[j];

                root = FMathUtil.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1);
                if (root < 0)
                {
                    throw new IndexOutOfRangeException("error!");
                }

                quat[i] = FFloat.Half * root;
                root = FFloat.Half / root;
                quat.W = (m[k, j] - m[j, k]) * root;
                quat[j] = (m[j, i] + m[i, j]) * root;
                quat[k] = (m[k, i] + m[i, k]) * root;
            }

            FFloat nor = FMathUtil.Sqrt(Dot(quat, quat));
            quat = new FQuaternion(quat.X / nor, quat.Y / nor, quat.Z / nor, quat.W / nor);

            return quat;
        }

        private static FMatrix33 LookRotationToMatrix(FVector3 viewVec, FVector3 upVec)
        {
            FVector3 z = viewVec;
            FMatrix33 m = new FMatrix33();

            FFloat mag = z.Magnitude;
            if (mag <= 0)
            {
                m = FMatrix33.Identity;
            }

            z /= mag;

            FVector3 x = FVector3.Cross(upVec, z);
            mag = x.Magnitude;
            if (mag <= 0)
            {
                m = FMatrix33.Identity;
            }

            x /= mag;

            FVector3 y = FVector3.Cross(z, x);

            m[0, 0] = x.X;
            m[0, 1] = y.X;
            m[0, 2] = z.X;
            m[1, 0] = x.Y;
            m[1, 1] = y.Y;
            m[1, 2] = z.Y;
            m[2, 0] = x.Z;
            m[2, 1] = y.Z;
            m[2, 2] = z.Z;

            return m;
        }

        #endregion

        #region operator

        public static FQuaternion operator *(FQuaternion lhs, FQuaternion rhs)
        {
            return new FQuaternion(lhs.W * rhs.X + lhs.X * rhs.W + lhs.Y * rhs.Z - lhs.Z * rhs.Y,
                lhs.W * rhs.Y + lhs.Y * rhs.W + lhs.Z * rhs.X - lhs.X * rhs.Z,
                lhs.W * rhs.Z + lhs.Z * rhs.W + lhs.X * rhs.Y - lhs.Y * rhs.X,
                lhs.W * rhs.W - lhs.X * rhs.X - lhs.Y * rhs.Y - lhs.Z * rhs.Z);
        }

        public static FVector3 operator *(FQuaternion rotation, FVector3 point)
        {
            FFloat _2x = rotation.X * 2;
            FFloat _2y = rotation.Y * 2;
            FFloat _2z = rotation.Z * 2;
            FFloat _2xx = rotation.X * _2x;
            FFloat _2yy = rotation.Y * _2y;
            FFloat _2zz = rotation.Z * _2z;
            FFloat _2xy = rotation.X * _2y;
            FFloat _2xz = rotation.X * _2z;
            FFloat _2yz = rotation.Y * _2z;
            FFloat _2xw = rotation.W * _2x;
            FFloat _2yw = rotation.W * _2y;
            FFloat _2zw = rotation.W * _2z;
            var x = (1 - (_2yy + _2zz)) * point.X + (_2xy - _2zw) * point.Y + (_2xz + _2yw) * point.Z;
            var y = (_2xy + _2zw) * point.X + (1 - (_2xx + _2zz)) * point.Y + (_2yz - _2xw) * point.Z;
            var z = (_2xz - _2yw) * point.X + (_2yz + _2xw) * point.Y + (1 - (_2xx + _2yy)) * point.Z;
            return FVector3.FromFFloat(x, y, z);
        }

        public static bool operator ==(FQuaternion lhs, FQuaternion rhs)
        {
            var isEqu = lhs.X == rhs.X && lhs.Y == rhs.Y && lhs.Z == rhs.Z && lhs.W == rhs.W;

            return isEqu;
        }

        public static bool operator !=(FQuaternion lhs, FQuaternion rhs)
        {
            return !(lhs == rhs);
        }

        #endregion
    }
}